#include "adxl345.h"

#define ADXL345_SPI_READ    0x80
#define ADXL345_SPI_WRITE   0x00
#define ADXL345_MULTI_BYTE  0x60

const uint8_t adxl_dummy_tx[6] = {
    0xff, 0xff, 0xff, 0xff, 0xff, 0xff
};

uint8_t adxl_rx_buf[6] = {0};

uint8_t adxl345_write_reg(ADXL345_HandleTypeDef* adxl345,
                          uint8_t reg_addr,
                          uint8_t reg_data);
uint8_t adxl345_read_reg(ADXL345_HandleTypeDef* adxl345, uint8_t reg_addr);
void adxl345_read_regs(ADXL345_HandleTypeDef* adxl345,
                       uint8_t reg_addr,
                       uint8_t* data,
                       uint8_t len);
uint8_t adxl345_nonblocking_init(ADXL345_HandleTypeDef* adxl);
void adxl345_handle_byte(void* pAdxl);

void adxl345_nonblocking_process(ADXL345_HandleTypeDef* adxl)
{
    switch (adxl->status) {
    case ADXL_INIT:
        if (ADXL_COMPLETE == adxl345_nonblocking_init(adxl))
            adxl->status = ADXL_READ_ACC;
        return;
    case ADXL_READ_ACC: {
        uint8_t cmd = ADXL345_SPI_READ | ADXL345_MULTI_BYTE |
                      (ADXL345_DATAX0_REG & 0x3f);
        adxl->p_imu = (struct imu_raw_data *)spmc_malloc(0);
        if (adxl->p_imu == NULL)
            return;
        spi_select(ADXL345_CS_PIN, 1);
        HAL_SPI_Transmit(adxl->spi, &cmd, 1, 1000);
        adxl->p_imu->timestamp = board_get_systime();
        adxl->rx_handler = adxl345_handle_byte;
        HAL_SPI_TransmitReceive_DMA(adxl->spi, adxl_dummy_tx,
                                    adxl->rx_byte, 6);
    }
    return;
    case ADXL_SET_FS:
        adxl->status = ADXL_READ_ACC;
        return;
    default:
        return;
    }
}

uint8_t adxl345_read_pid(ADXL345_HandleTypeDef* adxl)
{
    uint8_t response = 0;
    response = adxl345_write_reg(adxl,
                                 ADXL345_DEVID_REG | ADXL345_SPI_READ,
                                 0xff);
    return response;
}

uint8_t adxl345_write_reg(ADXL345_HandleTypeDef* adxl,
                          uint8_t reg_addr,
                          uint8_t reg_data)
{
    uint8_t temp_val = 0;
    uint8_t _reg_addr = reg_addr;
    uint8_t _reg_data = reg_data;
    spi_select(ADXL345_CS_PIN, 1);
    HAL_SPI_Transmit(adxl->spi, &_reg_addr, 1, 1000);
    HAL_SPI_TransmitReceive(adxl->spi, &_reg_data, &temp_val, 1, 1000);
    spi_select(ADXL345_CS_PIN, 0);
    return temp_val;
}

uint8_t adxl345_read_reg(ADXL345_HandleTypeDef* adxl, uint8_t reg_addr)
{
    return adxl345_write_reg(adxl, reg_addr | ADXL345_SPI_READ, 0xff);
}

void adxl345_read_regs(ADXL345_HandleTypeDef* adxl,
                       uint8_t reg_addr,
                       uint8_t* data,
                       uint8_t len)
{
    uint8_t address = reg_addr | ADXL345_SPI_READ | ADXL345_MULTI_BYTE;
    spi_select(ADXL345_CS_PIN, 1);
    HAL_SPI_Transmit(adxl->spi, &address, 1, 1000);
    HAL_SPI_Receive(adxl->spi, data, len, 1000);
    spi_select(ADXL345_CS_PIN, 0);
}

uint8_t adxl345_nonblocking_init(ADXL345_HandleTypeDef* adxl)
{
    static uint8_t delay_cnt = 0;

    switch (adxl->initStructure.status) {
    case ADXL_UNIDENTIFIED:
        if (delay_cnt++ > adxl->init_delay) {
            delay_cnt = 0;
            uint8_t pid = adxl345_read_pid(adxl);
            if (0xe5 == pid) {
                adxl->rx_byte = adxl_rx_buf;
                adxl->initStructure.status = ADXL_STANDBY;
            }
        }
        return ADXL_UNIDENTIFIED;
    case ADXL_STANDBY: {
        if (delay_cnt++ > adxl->init_delay) {
            delay_cnt = 0;
            adxl345_write_reg(adxl, ADXL345_POWER_CTL_REG, 0x00);
            adxl->initStructure.status = ADXL_SET_RATE;
        }
        return ADXL_STANDBY;
    }
    case ADXL_SET_RATE: {
        if (delay_cnt++ > adxl->init_delay) {
            delay_cnt = 0;
            uint8_t ctrl = adxl->initStructure.Power_Mode
                           | adxl->initStructure.Output_Data_Rate;
            adxl345_write_reg(adxl, ADXL345_BW_RATE_REG, ctrl);
            adxl->initStructure.status = ADXL_SET_FORMAT;
        }
        return ADXL_SET_RATE;;
    }
    case ADXL_SET_FORMAT: {
        if (delay_cnt++ > adxl->init_delay) {
            delay_cnt = 0;
            uint8_t ctrl = adxl->initStructure.Full_Res |
                           adxl->initStructure.Full_Scale;
            adxl345_write_reg(adxl, ADXL345_DATA_FORMAT_REG, ctrl);
            adxl->initStructure.status = ADXL_CHECK_FS;
        }
        return ADXL_SET_FORMAT;
    }
    case ADXL_CHECK_FS: {
        if (delay_cnt++ > adxl->init_delay) {
            delay_cnt = 0;
            uint8_t reg_content =
                adxl345_read_reg(adxl, ADXL345_DATA_FORMAT_REG);
            adxl->initStructure.Full_Res = reg_content & 0x08;
            adxl->initStructure.Full_Scale = reg_content & 0x03;
            adxl->initStructure.status = ADXL_ENABLE;
        }
    }
    return ADXL_CHECK_FS;
    case ADXL_ENABLE: {
        if (delay_cnt++ > adxl->init_delay) {
            delay_cnt = 0;
            adxl345_write_reg(adxl, ADXL345_POWER_CTL_REG, 0x08);
            adxl->initStructure.status = ADXL_COMPLETE;
        }
    }
    return ADXL_ENABLE;
    case ADXL_COMPLETE:
        return ADXL_COMPLETE;
    default:
        adxl->initStructure.status = ADXL_UNIDENTIFIED;
        return ADXL_UNIDENTIFIED;
    }
}

void adxl345_handle_byte(void *pAdxl)
{
    ADXL345_HandleTypeDef *adxl = (ADXL345_HandleTypeDef *)pAdxl;
    float scale = 1.0;
    if (adxl->initStructure.Full_Res == ADXL345_FULL_RES) {
        scale = 3.9;
    } else {
        if (adxl->initStructure.Full_Scale == ADXL345_FS_2G) {
            scale = 3.9;
        } else if (adxl->initStructure.Full_Scale == ADXL345_FS_4G) {
            scale = 7.8;
        } else if (adxl->initStructure.Full_Scale == ADXL345_FS_8G) {
            scale = 15.6;
        } else if (adxl->initStructure.Full_Scale == ADXL345_FS_16G) {
            scale = 31.2;
        }
    }

    adxl->p_imu->acc[0] = (int16_t)((0x00ff & adxl->rx_byte[1]) << 8 |
                           adxl->rx_byte[0]) * scale * GRAVITY;
    adxl->p_imu->acc[1] = (int16_t)((0x00ff & adxl->rx_byte[3]) << 8 |
                           adxl->rx_byte[2]) * scale * GRAVITY;
    adxl->p_imu->acc[2] = (int16_t)((0x00ff & adxl->rx_byte[5]) << 8 |
                           adxl->rx_byte[4]) * scale * GRAVITY;
    event_post(&event_queue, EVENT_IMU_DATA, (void *)(adxl->p_imu));
}
